{
  "cells": [
    {
      "cell_type": "markdown",
      "source": [
        "## Introduction to ARIA Python library\n",
        "\n",
        "This example shows the essential steps needed to connect to a robot or simulator, then continuously receive robot state information and send motion commands.\n",
        "\n",
        "To run the whole program in Jupyter, select \"Run All\" from the Cell menu. The output from each cell is shown below it.  When running, a cell will show `[*]` next to it.   To run each cell, use the down arrow key to select each cell, and press `Ctrl-Enter` to run the chunk.  Note that not all cells generate visible output. To edit a cell, double click on it.  To reset the notebook, choose \"Restart & Clear All Output\" from the Kernel menu, or click the Restart button in the toolbar and choose Cell, All Output, Clear to clear old output.\n",
        "\nTo use the MobileSim simulator, simply run MobileSim before evaluating the code in this notebook.  To connect to an AmigoBot, Pioneer3 or other robot with a wifi interface, make sure you are on the same network as the wifi interface.  (Otherwise ARIA will attempt to connect through a serial port connection as configured via command-line options from sys.argv or given in an ARIAARGS environment variable.)"
      ],
      "metadata": {}
    },
    {
      "cell_type": "markdown",
      "source": [
        "First, import the `AriaPy` Python module. (`AriaPy` will load the ARIA native library/DLL when loaded.)"
      ],
      "metadata": {
        "slideshow": {
          "slide_type": "slide"
        }
      }
    },
    {
      "cell_type": "code",
      "source": [
        "from AriaPy import *"
      ],
      "outputs": [],
      "execution_count": 1,
      "metadata": {
        "collapsed": false,
        "scrolled": true,
        "slideshow": {
          "slide_type": "subslide"
        }
      }
    },
    {
      "cell_type": "markdown",
      "source": [
        "Next, connect to the robot.  If any robots with wifi interfaces are found, you can select one to use.  If the MobileSim simulator is running, connect to that.  Otherwise try to connect to a robot via direct serial connection.  (You can also set parameters to configure the connection in `/etc/Aria.args` or by adding them to `sys.argv` before calling `connecToRobot()`)\n",
        "\nAn `ArRobot` object is returned, which provides the primary interface to communicating with the robot."
      ],
      "metadata": {}
    },
    {
      "cell_type": "code",
      "source": [
        "robot = Aria.connectToRobot()"
      ],
      "outputs": [
        {
          "output_type": "stream",
          "name": "stdout",
          "text": [
            "Aria: Searching Network for WiBox Interfaces...\n",
            "Aria: : No WiBoxes found.\n",
            "Aria: Connecting to robot...\n",
            "Aria: Connected.\n"
          ]
        }
      ],
      "execution_count": 2,
      "metadata": {
        "collapsed": false,
        "scrolled": true
      }
    },
    {
      "cell_type": "markdown",
      "source": [
        "Now we enable the motors and run the robot's [task cycle](file:///usr/local/Aria/docs/index.html#syncRobot).  This is a loop in which communication with the robot or simulator is maintained, data is received, and can call user-added \"task\" functions within this regular cycle.  This cycle is synchronized to the data updates received from the robot, so tasks will be called for every update.  (When you run this code, you will see the counter value included in the output below increment with each cycle.)"
      ],
      "metadata": {}
    },
    {
      "cell_type": "code",
      "source": [
        "print('Running...')\n",
        "robot.enableMotors()\n",
        "robot.runAsync()"
      ],
      "outputs": [
        {
          "output_type": "stream",
          "name": "stdout",
          "text": [
            "Running...\n"
          ]
        }
      ],
      "execution_count": 3,
      "metadata": {
        "collapsed": false,
        "scrolled": true
      }
    },
    {
      "cell_type": "markdown",
      "source": [
        "The `robot` object will now receive regular data updates from the robot on a regular 100 milisecond cycle.  This data includes the robot pose, and other data.  \n",
        "\n",
        "The pose is the robot's own estimated position based on its movements since starting up.  The pose contains a position on a cartesian coordinate system (X, Y), and a rotation or heading (Theta).  The origin of the coordinate system is  `getPose()` returns an `ArPose` object which contains some useful comparison and other functions. You can access the components as `x`, `y`, and `th` members or call `getX()`, `getY()`, and `getTh()`.  Some helpful math functions are included in the `ArMath` namespace as well.\n",
        "\n\n\n",
        "Let's define a function that will print the pose each time:\n",
        " "
      ],
      "metadata": {}
    },
    {
      "cell_type": "code",
      "source": [
        "def printRobotPos():\n",
        "  print('pose (%d) -- %s' % (robot.getCounter(), robot.getPose()))"
      ],
      "outputs": [],
      "execution_count": null,
      "metadata": {
        "collapsed": false,
        "outputHidden": false,
        "inputHidden": false
      }
    },
    {
      "cell_type": "markdown",
      "source": [
        "We can use `ArRobot`'s `addSensorInterpTask()` method to give a function to the `robot` object so it can automatically call it every 100ms cycle immediately after new position and other information updates are received from the robot.\n",
        "\n",
        "The first argument is the function to call.  The second one is a name for this task. You can\n",
        "choose any name you want. "
      ],
      "metadata": {}
    },
    {
      "cell_type": "code",
      "source": [
        "robot.addSensorInterpTask(printRobotPos, 'PrintData')"
      ],
      "outputs": [
        {
          "output_type": "execute_result",
          "execution_count": 4,
          "data": {
            "text/plain": [
              "<AriaPy.ArSyncTask; proxy of <Swig Object of type 'ArSyncTask *' at 0x7f5c380861e0> >"
            ]
          },
          "metadata": {}
        }
      ],
      "execution_count": 4,
      "metadata": {
        "collapsed": false,
        "scrolled": true
      }
    },
    {
      "cell_type": "markdown",
      "source": [
        "Let's also define a function to generate the motion we want the robot to perform. `setVel()` sets desired translational velocity and `setRotVel()` sets desired rotational velocity.  Together, these two components result in a curved trajectory.  \n",
        "\n",
        "We will use `addUserTask()` this time.  These tasks are called near the end of the robot cycle, rather than the beginning.\n"
      ],
      "metadata": {}
    },
    {
      "cell_type": "code",
      "source": [
        "def driveRobot():\n",
        "  print('drive (%d)' % (robot.getCounter()))\n",
        "  robot.setVel(300)\n",
        "  robot.setRotVel(20)\n",
        "\nrobot.addUserTask(driveRobot, 'DriveRobot')"
      ],
      "outputs": [],
      "execution_count": null,
      "metadata": {
        "collapsed": false,
        "scrolled": true
      }
    },
    {
      "cell_type": "markdown",
      "source": [
        "If you want to change a task function, you must also replace the task in `robot`.  \n",
        "Use `replaceSensorInterpTask()` and `replaceUserTask()` to do so, making sure\n",
        "that the name you give is the same as before. (If you don't plan on ever replacing or\n",
        "removing the task, you can omit the name, and AriaPy will choose a unique name based on\n",
        "some internal Python function information.)"
      ],
      "metadata": {}
    },
    {
      "cell_type": "code",
      "source": [
        "def driveRobot():\n",
        "    print 'new drive function (%d)' % (robot.getCounter())\n",
        "    robot.setVel(500)\n",
        "    roobt.setRotVel(35)\n",
        "\nrobot.replaceUserTask(driveRobot, 'DriveRobot')"
      ],
      "outputs": [],
      "execution_count": null,
      "metadata": {
        "collapsed": true
      }
    },
    {
      "cell_type": "markdown",
      "source": [
        "This user task will send velocity commands every cycle, but the robot controller will\n",
        "maintain any velocity command until a new command is received. So if you want to stop\n",
        "the robot, you must both remove any tasks that are sending commands, and stop the robot:"
      ],
      "metadata": {}
    },
    {
      "cell_type": "code",
      "source": [
        "robot.removeTask('DriveRobot')\n",
        "robot.stop()"
      ],
      "outputs": [],
      "execution_count": null,
      "metadata": {
        "collapsed": true
      }
    },
    {
      "cell_type": "markdown",
      "source": [
        "*Exercize:* Define a new function that sets different velocity values. Then use `replaceTask()` to replace the `'DriveRobot'` task. \n",
        "\n",
        "To do so in interactive Python (Jupyter or nteract), insert a new notebook code cell below this one, enter the code, and run the cell. (Or restart the notebook.)\n",
        "\n",
        "What happens when you set a rotational velocity of 0?  What happens with a translational velocity of 0?   What happens with a large translational velocity but a small rotational velocity? \n",
        "What happens with a large rotational velocity and a large translational velocity?"
      ],
      "metadata": {}
    }
  ],
  "metadata": {
    "kernelspec": {
      "name": "python2",
      "language": "python",
      "display_name": "Python 2"
    },
    "language_info": {
      "mimetype": "text/x-python",
      "nbconvert_exporter": "python",
      "name": "python",
      "pygments_lexer": "ipython2",
      "version": "2.7.12",
      "file_extension": ".py",
      "codemirror_mode": {
        "version": 2,
        "name": "ipython"
      }
    },
    "kernel_info": {
      "name": "python2"
    }
  },
  "nbformat": 4,
  "nbformat_minor": 0
}